#!/usr/bin/env python
# -*- coding: UTF-8 -*-
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from actionlib_msgs.msg import GoalStatus
import actionlib
import rospy


class NavTest():
    def __init__(self):
        rospy.init_node('endpub', anonymous=True)
        rospy.on_shutdown(self.shutdown)

        # Publisher to manually control the robot (e.g. to stop it)
        # 发布控制机器人的消息
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)

        # Subscribe to the move_base action server
        # 订阅move_base服务器的消息
        self.move_base = actionlib.SimpleActionClient(
            "move_base", MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait 60 seconds for the action server to become available
        # 60s等待时间限制
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move base server")

   
        pub = rospy.Publisher("move_base_simple/goal",
                              PoseWithCovarianceStamped, queue_size=10)
        endpose = PoseWithCovarianceStamped()
        endpose.pose.pose = Pose(Point(-0.2, -5.3, 0.06),
                                 Quaternion(0, 0, -1, 0))
        pub.publish(endpose)
        # Begin the main loop and run through a sequence of locations
        # 开始主循环
        # while not rospy.is_shutdown():
        #     # Set up the goal location
        #     # 设定目标点
        #     self.goal = MoveBaseGoal()
        #     self.goal.target_pose.pose = locations["end"]
        #     self.goal.target_pose.header.frame_id = 'map'
        #     self.goal.target_pose.header.stamp = rospy.Time.now()
        #     # Start the robot toward the next location
        #     # 向下一个位置进发
        #     self.move_base.send_goal(self.goal)

        #     # Allow 5 minutes to get there
        #     # 五分钟时间限制
        #     finished_within_time = self.move_base.wait_for_result(
        #         rospy.Duration(300))

        #     # Check for success or failure
        #     # 查看是否成功到达
        #     if not finished_within_time:
        #         self.move_base.cancel_goal()
        #         rospy.loginfo("Timed out achieving goal")
        #     else:
        #         state = self.move_base.get_state()
        #         if state == GoalStatus.SUCCEEDED:
        #             rospy.loginfo("Goal succeeded!")
        #             rospy.loginfo("State:" + str(state))
        #         else:
        #             rospy.loginfo(
        #                 "Goal failed with error code: ")

        #     # How long have we been running?
        #     # 运行所用时间
        #     running_time = rospy.Time.now() - start_time
        #     running_time = running_time.secs / 60.0
        #     # Print a summary success/failure, distance traveled and time elapsed
        #     # 输出本次导航的所有信息
        #     rospy.loginfo("Running time: " + str(trunc(running_time, 1)))

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.move_base.cancel_goal()
        rospy.sleep(2)
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)


def trunc(f, n):
    # Truncates/pads a float f to n decimal places without rounding
    slen = len('%.*f' % (n, f))
    return float(str(f)[:slen])


if __name__ == '__main__':
    NavTest()
    rospy.spin()
